using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using surfInterop;
using System.IO;
using System.Xml.Serialization;
using DotNetMatrix;
using System.Collections;


namespace Blimpbots.Recognition
{

    [Serializable]
    public struct EstimatedPoint
    {
        public string Name;
        public double Depth;
        public Point3D Position;
        public Point2D ImagePosition;
        public double Confidence;
        public double[] EllipseRotation;
        public EstimatedPoint(string name, Point3D pos, Point2D impos, double depth, double confidence)
        { Name = name; Position = pos; Confidence = confidence; ImagePosition = impos; Depth = depth; EllipseRotation = null; }
    }

    public class RecognitionModel
    {
        [XmlIgnore]
        //private Algorithms.SimpleAngleAverage _smoothAngle = new Blimpbots.Recognition.Algorithms.SimpleAngleAverage();
        private Algorithms.SimpleAngleFilter _smoothAngle = new Blimpbots.Recognition.Algorithms.SimpleAngleFilter(0, 6.28);
        public double UpdateSmoothAngle(double beta, double betaStd)
        {
            return  _smoothAngle.Update(beta, betaStd);
        }
        public double SmoothAngle { get { return _smoothAngle.Predict(1.0); } }

        [XmlIgnore]
        public Blimpbots.Recognition.Algorithms.SimpleAngleMedian sam = null;

        [XmlIgnore]
        private double _smoothDepth;
        private bool firstDepth = true;
        public double SmoothDepth { get { return _smoothDepth; } }
        public double UpdateSmoothDepth(double depth, double scaleDeviation)
        {
            if (firstDepth) { firstDepth = false; _smoothDepth = depth; return depth; }
            double wt = 1.0 / (4*scaleDeviation + 0.1);
            _smoothDepth = (wt * depth + _smoothDepth) / (1.0 + wt);
            return _smoothDepth;
        }

        [XmlIgnore]
        public LikelyObject LastMatch;

        public int TotalExampleCount
        {
            get { return Examples.Count + GenerativeExamples.Count; }
        }

        public RecognitionExample[] GetExamples()
        {
            RecognitionExample[] allExamples 
                = new RecognitionExample[Examples.Count + GenerativeExamples.Count];

            for (int i = 0; i < Examples.Count; i++)
                allExamples[i] = Examples[i];

            for (int i = 0; i < GenerativeExamples.Count; i++)
                allExamples[i + Examples.Count] = GenerativeExamples[i];

            return allExamples;
        }

        public RecognitionExample GetExample(int index)
        {
            if (index < Examples.Count)
                return Examples[index];
            else {
                index -= Examples.Count;
                if (index < GenerativeExamples.Count)
                    return GenerativeExamples[index];
                else
                    throw new ArgumentOutOfRangeException();
                }
        }


        // Informs outside world that the correlate examples
        // method has generated something
        public event EventHandler ModelReady;

        public class ModelReadyEventArgs : EventArgs
        {
            public ModelReadyEventArgs(AffineModel m, Example eA, Example eB) 
            { _m = m; _eA = eA; _eB = eB; }
            private AffineModel _m; private Example _eA, _eB;
            public AffineModel AffineMdl { get { return _m; } }
            public Example ExampleA { get { return _eA; } }
            public Example ExampleB { get { return _eB; } }
        }

        List<Example> _examples;
        public List<Example> Examples 
        {
            get { return _examples; }
            set { _examples = value; }
        }

        [XmlIgnore]
        public List<GenerativeExample> GenerativeExamples = new List<GenerativeExample>();

        public int LearnCorrespondences()
        {
            /* For each pair of examples in the model,
             *   match keypoints bi-directionally.  If the match agrees
             *   in both directions, add this to a match correspondence list.
             */

            /* Prepare arrays of lists of correspondences */
            for (int i = 0; i < _examples.Count; i++)
            {
                _examples[i].Correspondences = new List<CorrespondingKeypoint>[_examples[i].ModelKeypoints.Count];
                for (int j = 0; j < _examples[i].ModelKeypoints.Count; j++)
                    _examples[i].Correspondences[j] = new List<CorrespondingKeypoint>(2);
            }

            int Count = 0;

            double mRatio = 0.7;

            for (int i = 0; i < _examples.Count; i++)
            {
                Example a = _examples[i];
                IList<SurfKeypoint> aKeypoints = a.ModelKeypoints;
                Matching m_a = new Matching(aKeypoints);

                for (int j = i + 1; j < _examples.Count; j++)
                {
                    Example b = _examples[j];
                    IList<SurfKeypoint> bKeypoints = b.ModelKeypoints;

                    Matching m_b = new Matching(bKeypoints);

                    List<KeypointMatch> kpm = m_a.Match(bKeypoints, mRatio);
                    foreach (KeypointMatch k in kpm)
                    {
                        List<KeypointMatch> kpm2 = m_b.Match(new SurfKeypoint[] { aKeypoints[k.modelIdx] }, mRatio);
                        if (kpm2.Count == 1)
                        {
                            KeypointMatch k2 = kpm2[0];

                            if (k.dataIdx == k2.modelIdx)
                            {
                                Count++;
                                _examples[i].Correspondences[k.modelIdx].Add(new CorrespondingKeypoint(j, k2.modelIdx));
                                _examples[j].Correspondences[k2.modelIdx].Add(new CorrespondingKeypoint(i, k.modelIdx));
                            }
                        }
                    } // end foreach in kpm
                } // end for j
            } // end for i

            return Count;

        }




        public void CorrelateExamples()
        {
            // Algorithm to refine model based on multiple examples
            // jrpowers@umich.edu
            // March 2007

            // for every example pair
            //    match keypoints with
            //       overly generous method
            // run intensive RANSAC to find final correspondences
            // figure out distance for each keypoint match, store that off
            // figure out keypoints that show up multiple times, keep them

            // Second algorithm: do this across all keypoints, not just model

            // Prepare the arrays
            foreach (Example e in _examples)
            {
                e.KeypointStrength = new int[e.ModelKeypoints.Count];
                e.MaxMatchError = new double[e.ModelKeypoints.Count];
            }

            for (int i = 0; i < _examples.Count; i++)
            {
                Example a = _examples[i];
                IList<SurfKeypoint> aKeypoints = a.ModelKeypoints;

                for (int j = i + 1; j < _examples.Count; j++)
                {
                    Example b = _examples[j];

                    IList<SurfKeypoint> bKeypoints = b.ModelKeypoints;

                    Matching m = new Matching(aKeypoints);

                    // We hope the top N matches correspond, so we are zealous
                    //   and select them greedily.  We actually select 5 times
                    //   this to help ensure we found all matches.
                    List<KeypointMatch> matches = m.MatchZealous(bKeypoints,
                        5*Math.Min(aKeypoints.Count, bKeypoints.Count));

                    // Ask for a very good fit (15 pts * 10px off) before termination
                    double goodScore = 1.0 / 15;
                    double errThresh = 10.0;
                    int maxRuns = 300; // Try 300 times

                    AffineModel affine = RansacAffine.Run(matches, 3, goodScore, errThresh, maxRuns);

                    if (affine != null /* && affine.Score < 1.0/5 */)
                    {
                        // for 7 and above we're fairly confident this is
                        // a quite rigid fit.
                        if (affine.Inliers.Count > 6)
                        {
                            ModelReady(this, new ModelReadyEventArgs(affine, a, b));

                            foreach (KeypointMatch match in affine.Inliers)
                            {
                                // Handle the first-ever match for this keypoint
                                if (a.KeypointStrength[match.modelIdx] == 0)
                                    a.MaxMatchError[match.modelIdx] = match.distance;
                                if (b.KeypointStrength[match.dataIdx] == 0)
                                    b.MaxMatchError[match.dataIdx] = match.distance;

                                # region Update maximum allowable error for this keypoint
                                a.MaxMatchError[match.modelIdx] =
                                    Math.Min(match.distance, a.MaxMatchError[match.modelIdx]);
                                b.MaxMatchError[match.dataIdx] =
                                    Math.Min(match.distance, b.MaxMatchError[match.dataIdx]);
                                #endregion

                                a.KeypointStrength[match.modelIdx]++;
                                b.KeypointStrength[match.dataIdx]++;

                            }
                        }
                    }
                }
            }
        }

        public void Open(string name, string parentFolder)
        {
            _modelName = name;
            HasName = true;

            if (!Directory.Exists(parentFolder + name))
                throw new Exception("Cannot find the Model directory.");

            XmlSerializer xs = new XmlSerializer(typeof(Example));

            DirectoryInfo di =  new DirectoryInfo((parentFolder + name));
            foreach (FileInfo fi in di.GetFiles("*.xml"))
            {
                using (FileStream fs = new FileStream(fi.FullName, FileMode.Open))
                {
                    try
                    {
                        Example oldEx = (Example)xs.Deserialize(fs);
                        if (oldEx == null) continue;
                        string imgFilename = parentFolder + name + "\\" + oldEx.Name + ".jpg";
                        Bitmap bkg = (Bitmap)Bitmap.FromFile(imgFilename);
                        oldEx.Image = (Image)bkg;
                        _examples.Add(oldEx);
                        fs.Close();
                    }
                    catch
                    {
                        /* Some error TODO: report */
                    }
                }

            }

        }


        public void Save(string parentFolder)
        {
            string myDir = parentFolder + ModelName;

            if (!Directory.Exists(myDir))
            {
                try
                {
                    Directory.CreateDirectory(myDir);
                }
                catch
                {
                    throw new Exception("Failed to create model folder: Models\\" + ModelName); 
                }
            }

            XmlSerializer xs = new XmlSerializer(typeof(Example));

            /* Save all the examples */

            foreach (Example e in _examples)
            {
                string jpgPath = myDir + "\\" + e.Name + ".jpg";

                if (!File.Exists(jpgPath))
                {
                    using (FileStream fs = new FileStream(jpgPath, FileMode.Create))
                    {
                        fs.Write(e.RawImage, 0, e.RawImage.Length);
                        fs.Flush();
                        fs.Close();
                    }
                }

                string xmlPath = myDir + "\\" + e.Name + ".xml";

                using (FileStream fs = new FileStream(xmlPath, FileMode.Create))
                {
                    xs.Serialize(fs, e);
                    fs.Flush();
                    fs.Close();
                }
            }

        }

        public RecognitionModel()
        {
            HasName = false;
            _examples = new List<Example>();
        }

        public void AddExample(Example item)
        {
            _examples.Add(item);
        }

        public string ModelName
        {
            get { return _modelName; }
            set { _modelName = value; HasName = true; }
        }

        private string _modelName;
        public bool HasName;

        [Serializable]
        [XmlInclude(typeof(NamedPoint))]
        public class Example : RecognitionExample
        {
            IList<SurfKeypoint> _bkgKeypoints = new List<SurfKeypoint>();

            [XmlIgnore]
            public IList<SurfKeypoint> BackgroundKeypoints
            {
                get { return _bkgKeypoints; }
                set { _bkgKeypoints = value; }
            }

            public string s_BackgroundKeypoints
            {
                get { return SerializeKeypoints(_bkgKeypoints); }
                set { _bkgKeypoints = DeserializeKeypoints(value); }
            }

            public Dictionary<string, Point> GetPointDictionary()
            {
            Dictionary<string, Point> dict = new Dictionary<string, Point>();
            foreach (KeyValuePair<string, NamedPoint> kvp in NamedPoints)
            {
                dict[kvp.Key] = kvp.Value.Point;
            }
            return dict;
        }
            public void UpdateNamedPoints(Dictionary<string, Point> pts)
            {
                foreach (KeyValuePair<string, Point> kvp in pts)
                {
                    if (NamedPoints.ContainsKey(kvp.Key))
                        NamedPoints[kvp.Key].Point = kvp.Value;
                    else
                        NamedPoints[kvp.Key] = new NamedPoint(kvp.Value, 0);
                }
            }

            [XmlIgnore]
            Image _image;

            [XmlIgnore]
            byte[] _rawImage;

            private string _name;

            public string Name
            {
                get { return _name; }
                set { _name = value; }
            }

            public byte[] RawImage { get { return _rawImage; } }

            [XmlIgnore]
            public Image Image { get { return _image; } set { _image = value; } }

            public Example() { /* for serialization */  }

            public Example(Image image, byte[] rawImage, List<SurfKeypoint> surfKeypoints)
            {
                // Make up a unique name for the example
                _name = Guid.NewGuid().ToString("N");

                _rawImage = rawImage;
                _image = image;

                _bkgKeypoints = surfKeypoints;
                // Initially we don't know what is within the model
                _modelKeypoints = new List<SurfKeypoint>();
            }

            /// <summary> 
            /// Property created expressly for the XmlSerializer 
            /// </summary> 
            /// <remarks> 
            /// Note the XML Serialiazation attributes; they control what elements are named when this object is serialized. 
            /// </remarks> 
            [XmlArray("NamedPoints")]
            [XmlArrayItem("NamedPointsLine", Type = typeof(DictionaryEntry))]
            public DictionaryEntry[] _x_NamedPoints
            {
                get
                {
                    //Make an array of DictionaryEntries to return 
                    DictionaryEntry[] ret = new DictionaryEntry[NamedPoints.Count];
                    int i = 0;
                    DictionaryEntry de;
                    //Iterate through NamedPoints to load items into the array. 
                    foreach (KeyValuePair<string, NamedPoint> NamedPointsLine in NamedPoints)
                    {
                        de = new DictionaryEntry();
                        de.Key = NamedPointsLine.Key;
                        de.Value = NamedPointsLine.Value;
                        ret[i] = de;
                        i++;
                    }
                    return ret;
                }
                set
                {
                    NamedPoints.Clear();
                    for (int i = 0; i < value.Length; i++)
                    {
                        NamedPoints.Add((string)value[i].Key, (NamedPoint)value[i].Value);
                    }
                }
            } 
        }
    }
}
